Generalized Analysis and Improvement of the Consistency of EKF-based SLAM

In this work, we study the inconsistency problem of EKF-based SLAM from the perspective of observability. We analytically prove that when the Jacobians of the system and measurement models are evaluated at the latest state estimates during every time step, the linearized error-state system employed in the EKF has observable subspace of dimension higher than that of the actual, nonlinear, SLAM system. As a result, the covariance estimates of the EKF undergo reduction in directions of the state space where no information is available, which is a primary cause of the inconsistency. Based on these theoretical results, we propose a general framework for improving the consistency of EKF-based SLAM. In particular, we propose selecting the EKF linearization points in a way that ensures that the resulting linearized system model has an observable subspace of appropriate dimension. This can be achieved by calculating the filter Jacobians using the first-ever available estimates for each state variable. The resulting “First-Estimates Jacobian” (FEJ) EKF has been tested both in simulation and in real-world experiments, and is shown to significantly outperform the standard EKF both in terms of accuracy and consistency.